
#pragma once

#include "MT/array.h"
#include "MT/ors.h"
#include "util.h"

class ObjectMacros {
public:
	/**
	Creates a weighing machine at a specified position.
	Orientation is fixed, parameter may be added later.
	*/
	static void createScale(MT::String scaleName, MT::String scalePart1Name, MT::String scalePart2Name, MT::Array<ors::Body*> &bodies, MT::Array<ors::Shape*> &shapes, MT::Array<ors::Joint*> &joints, std::istream& is) {
		ors::Body *s1 = new ors::Body(bodies), *s2 = new ors::Body(bodies), *scalePart1 = new ors::Body(bodies), *scalePart2 = new ors::Body(bodies), *s3 = new ors::Body(bodies), *s4 = new ors::Body(bodies);
		s1->createInitialShape(ors::ShapeType::boxST); s2->createInitialShape(ors::ShapeType::boxST); s3->createInitialShape(ors::ShapeType::boxST); s4->createInitialShape(ors::ShapeType::boxST);
		scalePart1->createInitialShape(ors::ShapeType::boxST); scalePart2->createInitialShape(ors::ShapeType::boxST);
		s1->shapes(0)->index = shapes.N; shapes.append(s1->shapes(0)); s1->fixed = true;
		s2->shapes(0)->index = shapes.N; shapes.append(s2->shapes(0)); s2->shapes(0)->cont = true;
		scalePart1->shapes(0)->index = shapes.N; shapes.append(scalePart1->shapes(0)); scalePart1->shapes(0)->cont = true;
		scalePart2->shapes(0)->index = shapes.N; shapes.append(scalePart2->shapes(0)); scalePart2->shapes(0)->cont = true;
		s3->shapes(0)->index = shapes.N; shapes.append(s3->shapes(0));
		s4->shapes(0)->index = shapes.N; shapes.append(s4->shapes(0));

		AnyList ats;
		anyListRead(ats,is);
		if(!is.good()) HALT("scale '"<<scaleName<<"' read error: in ");

		ors::Transformation X;
		MT::String *sval = NULL;
		sval=anyListGet<MT::String>(ats, "X", 1);    if(sval) X.setText(*sval);

		double scaleHeight = 0.6;
		double scaleWidth = 0.6;
		double ropeHeight = 0.4;
		double maxSpan = 0.278;

		double jointAxis[] = {0, 1, 0};
		double color1[] = {0.3, 0.3, 1};
		
		double size1[] = {0.03, 0.03, 0.6, 1};
		s1->X.pos = X.pos; s1->X.pos.p[2] += scaleHeight*0.5f;
		s1->setMass(0.1);
		memmove(s1->shapes(0)->size, size1, 4*sizeof(double));
		memmove(s1->shapes(0)->color, color1, 3*sizeof(double));
		
		double size2[] = {scaleWidth, 0.02, 0.02, 1};
		s2->X.pos = X.pos; s2->X.pos.p[2] += scaleHeight;
		s2->name = scaleName;
		s2->setMass(1.4);
		memmove(s2->shapes(0)->size, size2, 4*sizeof(double));
		memmove(s2->shapes(0)->color, color1, 3*sizeof(double));
		s2->ats.append(anyNew<double>("maxSpan", maxSpan));
		s2->ats.append(anyNew<MT::String>("scalePart1", scalePart1Name));
		s2->ats.append(anyNew<MT::String>("scalePart2", scalePart2Name));
		double one=1; s2->ats.append(anyNew<double>("isScale", one));

		ors::HingeJoint *h1 = new ors::HingeJoint(joints, s1, s2);
		memmove(h1->axis, jointAxis, 3*sizeof(double));
		memmove(h1->anchor, s2->X.pos.p, 3*sizeof(double));
		h1->anchor[2] += 0.05;

		/*
		Now we have:
		=====o=====
			 |
			 |
			 |
		*/

		double size3[] = {0.01, 0.01, ropeHeight, 1};
		s3->X.pos = s2->X.pos; s3->X.pos.p[0] -= (scaleWidth*0.5f-0.01); s3->X.pos.p[2]-= ropeHeight*0.5;
		s3->setMass(0.01);
		memmove(s3->shapes(0)->size, size3, 4*sizeof(double));
		memmove(s3->shapes(0)->color, color1, 3*sizeof(double));

		ors::HingeJoint *h2 = new ors::HingeJoint(joints, s2, s3);
		memmove(h2->axis, jointAxis, 3*sizeof(double));
		memmove(h2->anchor, s3->X.pos.p, 3*sizeof(double));
		h2->anchor[2] += ropeHeight*0.5;

		s4->X.pos = s2->X.pos; s4->X.pos.p[0] += (scaleWidth*0.5f-0.01); s4->X.pos.p[2]-= ropeHeight*0.5;
		s4->setMass(0.01);
		memmove(s4->shapes(0)->size, size3, 4*sizeof(double));
		memmove(s4->shapes(0)->color, color1, 3*sizeof(double));

		ors::HingeJoint *h3 = new ors::HingeJoint(joints, s2, s4);
		memmove(h3->axis, jointAxis, 3*sizeof(double));
		memmove(h3->anchor, s4->X.pos.p, 3*sizeof(double));
		h3->anchor[2] += ropeHeight*0.5;

		/*
		Now we have:
		o====o====o
		|	 |    |
		     |
			 |
		*/

		double size4[] = {0.08, 0.08, 0.02, 1};
		scalePart1->X.pos = s3->X.pos; scalePart1->X.pos.p[2] -= ropeHeight*0.5;
		scalePart1->name = scalePart1Name;
		scalePart1->setMass(0.1);
		scalePart1->ats.append(anyNew<double>("isScalePart", one));
		memmove(scalePart1->shapes(0)->size, size4, 4*sizeof(double));
		memmove(scalePart1->shapes(0)->color, color1, 3*sizeof(double));

		ors::HingeJoint *h4 = new ors::HingeJoint(joints, s3, scalePart1);
		h4->type = ors::JointType::fixedJT;
		memmove(h4->axis, jointAxis, 3*sizeof(double));
		memmove(h4->anchor, scalePart1->X.pos.p, 3*sizeof(double));

		scalePart2->X.pos = s4->X.pos; scalePart2->X.pos.p[2] -= ropeHeight*0.5;
		scalePart2->name = scalePart2Name;
		scalePart2->setMass(0.1);
		scalePart2->ats.append(anyNew<double>("isScalePart", one));
		memmove(scalePart2->shapes(0)->size, size4, 4*sizeof(double));
		memmove(scalePart2->shapes(0)->color, color1, 3*sizeof(double));

		ors::HingeJoint *h5 = new ors::HingeJoint(joints, s4, scalePart2);
		h5->type = ors::JointType::fixedJT;
		memmove(h5->axis, jointAxis, 3*sizeof(double));
		memmove(h5->anchor, scalePart2->X.pos.p, 3*sizeof(double));

		/*
		And finally:
		o====o====o
		|	 |    |
	  __o__  |  __o__
			 |
		*/
	}
};